#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <iostream>
#include <iomanip>
#include <cmath>

class VelocityDisplayNode
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber odom_sub_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    
    nav_msgs::Odometry current_odom_;
    nav_msgs::Odometry previous_odom_;
    bool has_odom_data_;
    bool has_previous_odom_;
    ros::Time last_time_;
    double calculated_linear_velocity_;
    double calculated_angular_velocity_;

public:
    VelocityDisplayNode() : tf_listener_(tf_buffer_), has_odom_data_(false), has_previous_odom_(false), 
                           calculated_linear_velocity_(0.0), calculated_angular_velocity_(0.0)
    {
        // 订阅里程计话题
        odom_sub_ = nh_.subscribe("/odom", 10, &VelocityDisplayNode::odomCallback, this);
        
        ROS_INFO("速度显示节点已启动");
        ROS_INFO("通过里程计数据计算线速度和角速度");
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        // 保存上一次的里程计数据
        if (has_odom_data_)
        {
            previous_odom_ = current_odom_;
            has_previous_odom_ = true;
        }
        
        current_odom_ = *msg;
        has_odom_data_ = true;
        
        // 计算速度
        if (has_previous_odom_)
        {
            calculateVelocities();
        }
    }
    
    void calculateVelocities()
    {
        // 计算时间差
        double dt = (current_odom_.header.stamp - previous_odom_.header.stamp).toSec();
        
        if (dt > 0.0)
        {
            // 计算位置差
            double dx = current_odom_.pose.pose.position.x - previous_odom_.pose.pose.position.x;
            double dy = current_odom_.pose.pose.position.y - previous_odom_.pose.pose.position.y;
            
            // 计算线速度 (m/s)
            calculated_linear_velocity_ = sqrt(dx*dx + dy*dy) / dt;
            
            // 计算角度差
            tf2::Quaternion current_q(
                current_odom_.pose.pose.orientation.x,
                current_odom_.pose.pose.orientation.y,
                current_odom_.pose.pose.orientation.z,
                current_odom_.pose.pose.orientation.w
            );
            
            tf2::Quaternion previous_q(
                previous_odom_.pose.pose.orientation.x,
                previous_odom_.pose.pose.orientation.y,
                previous_odom_.pose.pose.orientation.z,
                previous_odom_.pose.pose.orientation.w
            );
            
            // 计算角度差
            tf2::Quaternion q_diff = current_q * previous_q.inverse();
            double yaw_diff = atan2(2.0 * (q_diff.w() * q_diff.z() + q_diff.x() * q_diff.y()),
                                  1.0 - 2.0 * (q_diff.y() * q_diff.y() + q_diff.z() * q_diff.z()));
            
            // 计算角速度 (rad/s)
            calculated_angular_velocity_ = yaw_diff / dt;
        }
    }

    void getCurrentPose()
    {
        try
        {
            geometry_msgs::TransformStamped transform;
            transform = tf_buffer_.lookupTransform("map", "base_link", ros::Time(0));
            
            // 获取位置
            double x = transform.transform.translation.x;
            double y = transform.transform.translation.y;
            double z = transform.transform.translation.z;
            
            // 获取四元数并转换为欧拉角
            tf2::Quaternion q(
                transform.transform.rotation.x,
                transform.transform.rotation.y,
                transform.transform.rotation.z,
                transform.transform.rotation.w
            );
            
            tf2::Matrix3x3 m(q);
            double roll, pitch, yaw;
            m.getRPY(roll, pitch, yaw);
            
            // 转换为度数
            double yaw_degrees = yaw * 180.0 / M_PI;
            
            // 显示信息
            std::cout << "\033[2J\033[1;1H"; // 清屏
            std::cout << "==================== 松灵小车传感器数据监控 ====================" << std::endl;
            std::cout << "时间: " << ros::Time::now() << std::endl;
            std::cout << "------------------------------------------------------------" << std::endl;
            
            if (has_odom_data_)
            {
                std::cout << "计算得出的速度:" << std::endl;
                std::cout << "  线速度: " << std::fixed << std::setprecision(3) << calculated_linear_velocity_ << " m/s" << std::endl;
                std::cout << "  角速度: " << std::fixed << std::setprecision(3) << calculated_angular_velocity_ << " rad/s" << std::endl;
            }
            else
            {
                std::cout << "线速度: 暂无数据" << std::endl;
                std::cout << "角速度: 暂无数据" << std::endl;
            }
            
            std::cout << "------------------------------------------------------------" << std::endl;
            std::cout << "位置坐标 (m):" << std::endl;
            std::cout << "  X: " << std::fixed << std::setprecision(3) << x << std::endl;
            std::cout << "  Y: " << std::fixed << std::setprecision(3) << y << std::endl;
            std::cout << "  Z: " << std::fixed << std::setprecision(3) << z << std::endl;
            
            std::cout << "旋转角度 (度):" << std::endl;
            std::cout << "  横滚角 (Roll): " << std::fixed << std::setprecision(2) << roll * 180.0 / M_PI << std::endl;
            std::cout << "  俯仰角 (Pitch): " << std::fixed << std::setprecision(2) << pitch * 180.0 / M_PI << std::endl;
            std::cout << "  偏航角 (Yaw): " << std::fixed << std::setprecision(2) << yaw_degrees << std::endl;
            
            if (has_odom_data_)
            {
                std::cout << "------------------------------------------------------------" << std::endl;
                std::cout << "里程计数据:" << std::endl;
                std::cout << "  线速度: " << std::fixed << std::setprecision(3) 
                         << current_odom_.twist.twist.linear.x << " m/s" << std::endl;
                std::cout << "  角速度: " << std::fixed << std::setprecision(3) 
                         << current_odom_.twist.twist.angular.z << " rad/s" << std::endl;
            }
            
            std::cout << "============================================================" << std::endl;
            std::cout << "按 Ctrl+C 退出程序" << std::endl;
        }
        catch (tf2::TransformException &ex)
        {
            std::cout << "\033[2J\033[1;1H"; // 清屏
            std::cout << "==================== 松灵小车传感器数据监控 ====================" << std::endl;
            std::cout << "时间: " << ros::Time::now() << std::endl;
            std::cout << "------------------------------------------------------------" << std::endl;
            std::cout << "错误: 无法获取位置信息 - " << ex.what() << std::endl;
            std::cout << "请确保TF变换已正确发布" << std::endl;
            std::cout << "============================================================" << std::endl;
        }
    }

    void run()
    {
        ros::Rate rate(10); // 10Hz
        
        while (ros::ok())
        {
            getCurrentPose();
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "velocity_display_node");
    
    // 设置控制台编码为UTF-8以正确显示中文
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    VelocityDisplayNode node;
    node.run();
    
    return 0;
}
